#include <cstdio>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "ros/console.h"
#include "nav_msgs/GetMap.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "geometry_msgs/Quaternion.h"
#include <alibabacloud/oss/OssClient.h>
#include <iostream>
#include <sstream>
using namespace AlibabaCloud::OSS;
using namespace std;
string url;
std_msgs::String result;
unsigned int fileNum = 1;
char key;
bool mapSaveFlag;
class MapGenerator
{

  public:
    MapGenerator(const std::string& mapname, int threshold_occupied, int threshold_free)
      : mapname_(mapname), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free)
    {
      ros::NodeHandle n;
      sub = n.subscribe("/read", 1000, &MapGenerator::Callback,this);        
      map_sub_ = n.subscribe("map", 1000, &MapGenerator::mapCallback, this);//
      getmap_pub = n.advertise<std_msgs::String>("/read_upmap", 1000);
      upmap_read = n.subscribe("/read_getmap",1000,&MapGenerator::upmapCallback,this);
      read_pub = n.advertise<std_msgs::String>("/write", 1000);
    }

    void Callback(const std_msgs::String::ConstPtr &msg){
      ROS_INFO("Waiting for the map1");
      ROS_INFO_STREAM("I heard: " << msg->data);
      if (msg->data == "s")   //6
     {
         key = 32;        
     }
     else 
     {
         key = 0;
     }
    }

    void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
    {
        	if(key == 32)
			mapSaveFlag = true;
                key = 0;
                if(mapSaveFlag)
		{
			mapSaveFlag = false;
		        ROS_INFO("Waiting for the map2");
			ROS_INFO("Received a %d X %d map @ %.3f m/pix",
			       map->info.width,
			       map->info.height,
			       map->info.resolution);


		      std::string mapdatafile = "/home/liang/map/oldmap/" + mapname_ + ".pgm";  //.pgm
		      ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
		      FILE* out = fopen(mapdatafile.c_str(), "w");
		      if (!out)
		      {
			ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
			return;
		      }

		      fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
			      map->info.resolution, map->info.width, map->info.height);
		      for(unsigned int y = 0; y < map->info.height; y++) {
			for(unsigned int x = 0; x < map->info.width; x++) {
			  unsigned int i = x + (map->info.height - y - 1) * map->info.width;
			  if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free)
			    fputc(254, out);
			  } else if (map->data[i] >= threshold_occupied_) { // (occ,255]
			    fputc(000, out);
			  } else { //occ [0.25,0.65]
			    fputc(205, out);
			  }
			}
		      }

		      fclose(out);

		      std::string mapmetadatafile = "/home/liang/map/oldmap/" + mapname_ + ".yaml";
		      ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
		      FILE* yaml = fopen(mapmetadatafile.c_str(), "w");

		      geometry_msgs::Quaternion orientation = map->info.origin.orientation;
		      tf2::Matrix3x3 mat(tf2::Quaternion(
			orientation.x,
			orientation.y,
			orientation.z,
			orientation.w
		      ));
		      double yaw, pitch, roll;
		      mat.getEulerYPR(yaw, pitch, roll);

		      fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
			      mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw);

		      fclose(yaml);

		      ROS_INFO("Done\n");
		      result.data = "1";
		      getmap_pub.publish(result);
      		}
      
    
    }

    void upmapCallback(const std_msgs::String::ConstPtr &msg){
      ROS_INFO("Waiting for the map3");
      ROS_INFO_STREAM("I heard: " << msg->data);
      if (msg->data == "1")   //1
     {
	string fp = "/home/liang/map/newmap/newmap.jpg";
        int a = uploadimg(fp); //
        if(a == -1){
		cout<<"fail!!"<<endl;
		result.data = "-1";
		read_pub.publish(result);
		return;
        }  
        result.data = url;
        read_pub.publish(result);
        cout << "map has been uploaded." << endl;
     }
      
    }

    std::string mapname_;
    ros::Subscriber map_sub_;
    ros::Subscriber sub;
    ros::Publisher getmap_pub;
    ros::Subscriber upmap_read;
    ros::Publisher read_pub;
    int threshold_occupied_;
    int threshold_free_;
    
    int uploadimg(string a)
	{
		/* 初始化OSS账号信息 */
		string AccessKeyId = "LTAI4FxrnjrBYKebVCGhqRgZ";
		string AccessKeySecret = "DHbaZGdyYmqvbiMFITZgRbkkK3aQXd";
		string Endpoint = "http://oss-cn-hangzhou.aliyuncs.com";
		string BucketName = "photo202001";
		/* yourObjectName表示上传文件到OSS时需要指定包含文件后缀在内的完整路径，例如abc/efg/123.jpg */
		/*string ObjectName = a; */
                stringstream filePath;
                filePath << "Map" << "/" << fileNum << ".jpg";
                string ObjectName = filePath.str();//不能以/开头
		string GetobjectUrlName = filePath.str();
                fileNum++;
		/* 初始化网络等资源 */
		InitializeSdk();

		ClientConfiguration conf;
		OssClient client(Endpoint, AccessKeyId, AccessKeySecret, conf);

		/* 上传文件 */
		/* yourLocalFilename由本地文件路径加文件名包括后缀组成，例如/users/local/myfile.txt.*/

		auto outcome = client.PutObject(BucketName, ObjectName,a);   
		
		if (!outcome.isSuccess()) {
		                /* 异常处理 */
		                cout << "PutObject fail" <<
		                ",code:" << outcome.error().Code() <<
		                ",message:" << outcome.error().Message() <<
		                ",requestId:" << outcome.error().RequestId() <<endl;
		                ShutdownSdk();
		                return -1;
		}

		/* 设置签名URL有效时间*/
		time_t t = std::time(nullptr) + 1200;
		/* 生成下载文件URL */
		auto genOutcome = client.GeneratePresignedUrl(BucketName, GetobjectUrlName, t, Http::Get);
		if (genOutcome.isSuccess()) {
		                cout << "GeneratePresignedUrl success, Gen url:" << genOutcome.result().c_str() << endl;
		                url = genOutcome.result().c_str();
		}
		else {
		                /* 异常处理 */
		                cout << "GeneratePresignedUrl fail" <<
		                ",code:" << genOutcome.error().Code() <<
		                ",message:" << genOutcome.error().Message() <<
		                ",requestId:" << genOutcome.error().RequestId() << endl;
		                ShutdownSdk();
		                return -1;
		}

		/* 释放网络等资源 */
		ShutdownSdk();
		return 1;
	}
};

#define USAGE "Usage: \n" \
              "  map_saver -h\n"\
              "  map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] [ROS remapping args]"




int main(int argc, char** argv)
{
  ros::init(argc, argv, "map_saver");
  std::string mapname = "map";  
  int threshold_occupied = 65;
  int threshold_free = 25;
  for(int i=1; i<argc; i++)
  {
    if(!strcmp(argv[i], "-h"))
    {
      puts(USAGE);
      return 0;
    }
    else if(!strcmp(argv[i], "-f"))
    {
      if(++i < argc)
        mapname = argv[i];
      else
      {
        puts(USAGE);
        return 1;
      }
    }
    else if (!strcmp(argv[i], "--occ"))
    {
      if (++i < argc)
      {
        threshold_occupied = std::atoi(argv[i]);
        if (threshold_occupied < 1 || threshold_occupied > 100)
        {
          ROS_ERROR("threshold_occupied must be between 1 and 100");
          return 1;
        }

      }
      else
      {
        puts(USAGE);
        return 1;
      }
    }
    else if (!strcmp(argv[i], "--free"))
    {
      if (++i < argc)
      {
        threshold_free = std::atoi(argv[i]);
        if (threshold_free < 0 || threshold_free > 100)
        {
          ROS_ERROR("threshold_free must be between 0 and 100");
          return 1;
        }

      }
      else
      {
        puts(USAGE);
        return 1;
      }
    }
    else
    {
      puts(USAGE);
      return 1;
    }
  }

  if (threshold_occupied <= threshold_free)
  {
    ROS_ERROR("threshold_free must be smaller than threshold_occupied");
    return 1;
  }

  MapGenerator mg(mapname, threshold_occupied, threshold_free);

  while(ros::ok())
    ros::spinOnce();

  return 0;
}
